//
// Created by lixiao on 2022/3/29.
//

#include <iostream>
#include <boost/signals2/signal.hpp>
#include <boost/bind/bind.hpp>
#include <chrono>
#include <memory>

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include "std_msgs/msg/int32.hpp"
#include "example_interfaces/srv/add_two_ints.hpp"

using namespace std::chrono_literals;



void add(const std::shared_ptr<example_interfaces::srv::AddTwoInts::Request> request,
         std::shared_ptr<example_interfaces::srv::AddTwoInts::Response>      response)
{
  response->sum = request->a + request->b;
  RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Incoming request\na: %ld" " b: %ld",
              request->a, request->b);
  RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "sending back response: [%ld]", (long int)response->sum);
}

class MinimalPublisher : public rclcpp::Node
{
public:
    MinimalPublisher()
            : Node("minimal_publisher"), count_(0)
    {
      publisher_ = this->create_publisher<std_msgs::msg::Int32>("topic", 10);
      timer_ = this->create_wall_timer(
              500ms, std::bind(&MinimalPublisher::timer_callback, this));
    }

private:
    void timer_callback()
    {
      auto message = std_msgs::msg::Int32();
      message.data = count_++;
      publisher_->publish(message);
    }
    rclcpp::TimerBase::SharedPtr timer_;
    rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr publisher_;
    size_t count_;
};

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  std::shared_ptr<rclcpp::Node> srvnode = rclcpp::Node::make_shared("add_two_ints_server");

  rclcpp::Service<example_interfaces::srv::AddTwoInts>::SharedPtr service =
          srvnode->create_service<example_interfaces::srv::AddTwoInts>("add_two_ints", &add);

  RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Ready to add two ints.");
  rclcpp::executors::MultiThreadedExecutor executor;
  auto pubnode = std::make_shared<MinimalPublisher>();


  executor.add_node(pubnode);
  executor.add_node(srvnode);
  executor.spin();
  rclcpp::shutdown();
  return 0;
}




















//namespace gui{
//
//class Interface{
//  public:
//      virtual void print_args(float x, float y) = 0; // pure virtual function
//      virtual void heartbeat() = 0;
//  };
//
//}
//
//class InterfaceImpl : gui::Interface{
//public:
//  void print_args(float x, float y) override ;
//  void heartbeat();
//
//};
//
//void InterfaceImpl::print_args(float x, float y) {
//  std::cout << "InterfaceImpl: The arguments are " << x << " and " << y << std::endl;
//}
//
//void InterfaceImpl::heartbeat() {
//  std::cout << "InterfaceImpl: Heartbeat! " << std::endl;
//}
//
//
//int main(int argc, char * argv[])
//{
//
//  boost::signals2::signal<void (float, float)> sig;
//  boost::signals2::signal<void ()> sig2;
//  InterfaceImpl impl;
//
//  sig.connect(boost::bind(&InterfaceImpl::print_args,&impl,
//          _1,_2));
//  sig2.connect(boost::bind(&InterfaceImpl::heartbeat,&impl));
//  sig2();
//  sig(5., 3.);
//  return 0;
//}